/* Based on Density1D.c
Estimates density of individuals, mean trait values, and trait variances at each individual's location
Assumes a Gaussian kernel as the neighbourhood size.
Requires an array of 1D locations (X[i]), trait values (H[i], D[i])
bandwidth currently fixed at bw=1 (easy enough to make variable)
*/

#include <R.h>
#include <Rinternals.h>
//#include <Rmath.h>
//#include <Rdefines.h>
#include <math.h>
#include "Rinterface.h"


#define Pi 3.141593



//--------------------------------------//
// Function declarations //

double norm (double x, double bw);
SEXP sum_metrics (SEXP R_X, SEXP R_H, SEXP R_D, SEXP R_n, SEXP R_bins, SEXP R_nbins, SEXP R_spX, SEXP R_bw);


//--------------------------------------//
//  Function definitions  //

double norm (double x, double bw){
	return exp(-pow(x,2)/(2*pow(bw,2)))/(bw*sqrt(2*Pi));
}


// A serial version of the metrics function that calculates metrics back to fixed points rather than individuals
SEXP sum_metrics (SEXP R_X, SEXP R_H, SEXP R_D, SEXP R_n, SEXP R_bins, SEXP R_nbins, SEXP R_spX, SEXP R_bw){
	int n = INTEGER(coerceVector(R_n, INTSXP))[0]; //grab vector length
	int nbins = INTEGER(coerceVector(R_nbins, INTSXP))[0]; //grab bin length
	int spX = INTEGER(coerceVector(R_spX, INTSXP))[0]; //grab spaceX
	int bw = INTEGER(coerceVector(R_bw, INTSXP))[0];
	
	R_X=coerceVector(R_X, REALSXP); //digest R_X
	R_H=coerceVector(R_H, REALSXP); //digest R_H
	R_D=coerceVector(R_D, REALSXP); //digest R_D
	R_bins=coerceVector(R_bins, INTSXP);
	SEXP outmat; PROTECT(outmat=allocMatrix(REALSXP, nbins, 5)); // a place to put all the output
	
	double *X, *RH, *RD, *out;  //pointers variables
	double w, r; //to take weights
	int *Rb, ii, jj; // iterations variables
	
	
	r = (2*spX)/(2*Pi);
	X = REAL(R_X); //pointers to real parts of R vectors
	RH = REAL(R_H);
	RD = REAL(R_D);
	out = REAL(outmat);
	Rb = INTEGER(R_bins);
		
	//calculate density and mean trait values
	for (ii=0; ii<nbins; ii++){
		out[ii] = 0; //column 1 for density
		out[ii+nbins] = 0; //column 2 for meanD
		out[ii+2*nbins] = 0; //column 3 for meanH
		for (jj=0; jj<n; jj++){
			w = norm((acos(cos((Rb[ii]-X[jj])/r)))*r, bw);
			out[ii] += w;
			out[ii+nbins] += w*(RD[jj]);
			out[ii+2*nbins] += w*(RH[jj]);
		}
		out[ii+nbins] = out[ii+nbins]/out[ii];
		out[ii+2*nbins] = out[ii+2*nbins]/out[ii];
	}
	
	//calculate trait variances
			for (ii=0; ii<nbins; ii++){
				out[ii+3*nbins] = 0;
				out[ii+4*nbins] = 0;
				for (jj=0; jj<n; jj++){
					w = norm((acos(cos((Rb[ii]-X[jj])/r)))*r, bw);
					out[ii+3*nbins] += w*pow(out[ii+nbins]-RD[jj], 2);
					out[ii+4*nbins] += w*pow(out[ii+2*nbins]-RH[jj], 2);
				}
				out[ii+3*nbins] = sqrt(out[ii+3*nbins]/out[ii]);
				out[ii+4*nbins] = sqrt(out[ii+4*nbins]/out[ii]);
			}
	
	UNPROTECT(1);
	return(outmat);
}


